#include <dispatch_system/vehicle.h>
#include <float.h>
#include <commu_with_sm/system_manager_commu.hpp>
#include <sch_common/sch_common.h>


namespace moying
{
namespace navigation
{
    std::vector<MovePattern> Vehicle::move_pattern_vec_;

    Vehicle::Vehicle()
    {
        init();
    }

    Vehicle::Vehicle(std::string type, const std::string &name)
    :MapObject(type, name), id_(0),robot_type_(type)
    {
        init();
    }
    Vehicle::Vehicle(std::string type, const std::string &name, unsigned int id)
    :MapObject(type, name), id_(id),robot_type_(type)
    {
        init();
    }

    Vehicle::~Vehicle()
    {
        
    }

    void Vehicle::init(){
        nav_mode_info_.nav_mode = moying_proto::kNavWaitTimeout;
        nav_mode_info_.nav_wait_timeout = 3;
        length_     = 1;
        width_      = 0.618;
        height_     = 0.3;
    }

    void Vehicle::set_nav_mode_info(NavModeInfo info){
        nav_mode_info_ = info;
        if(nav_mode_info_.nav_mode == moying_proto::kNavWaitInfinite){
            nav_mode_info_.nav_wait_timeout = -1;
        }
    }

    bool Vehicle::isOmniVehicle(){
        return false;
    }

    void Vehicle::setMovePatterns()
    {
        ERROR("ERROR calling setMovePatterns() directly in vehicle base class");
    }

    MovePattern Vehicle::getMovePattern(MovePatternType move_pattern_type){

        if(move_pattern_vec_.empty()){

            sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
            float path_plan_map_res = cfg_info.traffic_manager_cfg.path_plan_map_resolution;
            float robot_nav_speed = cfg_info.traffic_manager_cfg.robot_nav_speed;
            robot_nav_speed = (robot_nav_speed <= 0 || robot_nav_speed >= 3) ? 0.5 : robot_nav_speed;
            path_plan_map_res = (path_plan_map_res <= 0 || path_plan_map_res > 1) ? 0.5 : path_plan_map_res;
            float time_res = 1/(robot_nav_speed/path_plan_map_res);

            int t = ceil(1/time_res);
            move_pattern_vec_.push_back(MovePattern(1, 0, 0, t, t, kMoveForwardState)); // 匀加速
            // t = ceil(2/time_res);
            // t=3;
            move_pattern_vec_.push_back(MovePattern(1, 0, 0, t, t, kStaticState)); // 匀减速

            move_pattern_vec_.push_back(MovePattern(0, 0, 0, 1, 1, kStaticState));  // 等待
            // t = ceil(2/time_res);
            t = ceil(2/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, 1, t, t+2, kStaticState));  // 左转45
            // t = ceil(4/time_res);
            t = ceil(3/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, 2, t, t+2, kStaticState));  // 左转90
            // t = ceil(5/time_res);
            t = ceil(4/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, 3, t, t+2, kStaticState));  // 左转135
            // t = ceil(6/time_res);
            t = ceil(5/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, 4, t, t+2, kStaticState));  // 左转180
            // t = ceil(2/time_res);
            t = ceil(2/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, -1, t, t+2,kStaticState)); // 右转45
            // t = ceil(4/time_res);
            t = ceil(3/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, -2, t, t+2,kStaticState)); // 右转90
            // t = ceil(5/time_res);
            t = ceil(4/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 0, -3, t, t+2,kStaticState)); // 右转135

            move_pattern_vec_.push_back(MovePattern(1, 0,   0, 1, 2, kMoveForwardState)); // 匀速
            move_pattern_vec_.push_back(MovePattern(0, 1,   0, 1, 5 , kLeftMoveShiftState)); // 左平移
            move_pattern_vec_.push_back(MovePattern(0, -1,  0, 1, 5, kRightMoveShiftState)); // 右平移
            move_pattern_vec_.push_back(MovePattern(1, 1,   0, 2, 10 , kMoveForwardState)); // 左前方
            move_pattern_vec_.push_back(MovePattern(1, -1,  0, 2, 10, kMoveForwardState)); // 右前方

            // t = ceil(2/time_res);
            t=2;
            move_pattern_vec_.push_back(MovePattern(0, 1,   0, t, 20, kLeftMoveShiftState)); // 左平移加速
            move_pattern_vec_.push_back(MovePattern(0, -1,  0, t, 20, kRightMoveShiftState)); // 右平移加速
            move_pattern_vec_.push_back(MovePattern(0, 1,   0, t, 20, kStaticState)); // 左平移减速
            move_pattern_vec_.push_back(MovePattern(0, -1,  0, t, 20, kStaticState)); // 右平移减速

            //弧线转弯pattern
            t = ceil(1/time_res);
            move_pattern_vec_.push_back(MovePattern(0, 1,  2, t, 8, kMoveForwardState)); // 左转弯
            move_pattern_vec_.push_back(MovePattern(0, -1, -2,t, 8, kMoveForwardState)); // 右转弯 

            //后退
            t=2;
            move_pattern_vec_.push_back(MovePattern(-1, 0, 0, t, 6, kMoveBackwardState)); // 匀加速后退
            move_pattern_vec_.push_back(MovePattern(-1, 0, 0, t, 6, kStaticState)); // 匀减速后退
            move_pattern_vec_.push_back(MovePattern(-1, 0, 0, 1, 10, kMoveBackwardState)); // 匀速后退

        }

        int index = (int)move_pattern_type;
        if(index < move_pattern_vec_.size()){
            return move_pattern_vec_[index];
        }else{
            return MovePattern(0, 0, 0, 0, 0, MovePatternState::kStaticState);
        }
    }


    MovePatternsPtr Vehicle::getValidMovePatterns(MovePatternState sm_state)
    {
        return move_patterns_[sm_state];
    }

    bool Vehicle::isMovePatternValid(MovePatternType type,MovePatternState sm_state){
        if( move_patterns_.find(sm_state) == move_patterns_.end() )
            return false;
        auto patterns = move_patterns_[sm_state];
        if(patterns == nullptr)
            return false;
        for(auto pattern : *patterns){
            if(type == pattern)
                return true;
        }
        return false;
    }

    double Vehicle::getTheta(int dir)
    {
        switch(dir)
        {
            case 0:
                return 0.0;
            case 1:
                return M_PI/2.0;
            case 2:
                return M_PI;
            case 3:
                return -M_PI/2.0;
            default:
                ERROR("Unknown theta direction "<<dir);
                return 0;
        }
        return 0;
    }
    
    int Vehicle::getCloseTheta(double th)
    {
        // std::vector<double> thetas= {0.0, M_PI/2.0, M_PI, -M_PI/2.0};
        std::vector<double> thetas= {0.0, M_PI_4, M_PI_2, M_PI*3.0/4.0,M_PI, -M_PI*3.0/4.0 , -M_PI_2, -M_PI_4};
        double diff_min = 1e10;
        int idx = 0;
        for(int i = 0; i < thetas.size(); i++)
        {
            double diff = fabs(th - thetas[i]);
            if(diff > M_PI){
                diff = M_PI*2-diff; 
            }
            if(diff < diff_min)
            {
                diff_min = diff;
                idx = i;
            }
        }
        return idx;
    }
    
    double Vehicle::getVelocity(int sm)
    {
        sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
        float robot_nav_speed = cfg_info.traffic_manager_cfg.robot_nav_speed;
        robot_nav_speed = (robot_nav_speed <= 0 || robot_nav_speed >= 3) ? 0.5 : robot_nav_speed;

        double ret = 0;
        switch(sm)
        {
            case 0:
                ret = 0;
                break;
            case 1:
                ret = robot_nav_speed;
                break;
            default:
                ret = 0;
                break;
        }
        return ret;
    }

    bool Vehicle::getUnpaddedFootprintOccupiedCells(const CostMapPtr &costmap, int x, int y, std::map<int, FootprintCell> &footprint_cells)
    {
        if(costmap == nullptr)
            return false;
        int half_length = ceil(length_/costmap->getResolution()/2);
        int half_width = ceil(width_/costmap->getResolution()/2);
        int length = ceil(length_/costmap->getResolution()), width = ceil(width_/costmap->getResolution());
        int sx = costmap->getSizeInCellsX(), sy = costmap->getSizeInCellsY();
        int idx;
        bool is_inflate;

        std::map<int, FootprintCell>::iterator iter;
        for(int xx = -length + 1; xx < length; xx++)
        {
            for(int yy = -width + 1; yy < width; yy++)
            {
                int nx = x + xx, ny = y + yy;
                if(nx >=0 && nx < sx && ny >=0 && ny < sy)
                {
                    idx = costmap->toIndex(nx, ny);
                    iter = footprint_cells.find(idx);
                    is_inflate = abs(xx) > half_length || abs(yy) > half_width;
                    if(iter == footprint_cells.end())
                    {
                        footprint_cells[idx] = FootprintCell(nx, ny, is_inflate);
                    }
                    else if(!is_inflate)
                    {
                        iter->second.inflate = false;
                    }
                }
            }
        }
        return true;
    }

    bool Vehicle::getFootprintOccupiedCells(const CostMapPtr &costmap, std::map<int, FootprintCell> &footprint_cells){

        if(costmap == nullptr)
            return false;
        int x = 0;
        int y = 0;
        if( !costmap->worldToMap(state_.x,state_.y,x,y) )
            return false;
        getFootprintOccupiedCells(costmap,x,y,footprint_cells,state_.th);
    }

    bool Vehicle::getFootprintOccupiedCells(const CostMapPtr &costmap, int x, int y, std::map<int, FootprintCell> &footprint_cells,float theta)
    {
        if(costmap == nullptr)
            return false;

        int half_length = ceil(length_/costmap->getResolution()/2);
        int half_width = ceil(width_/costmap->getResolution()/2);
        int length = ceil(length_/costmap->getResolution()), width = ceil(width_/costmap->getResolution());
        int sx = costmap->getSizeInCellsX(), sy = costmap->getSizeInCellsY();
        int idx;
        bool is_inflate;

        std::map<int, FootprintCell>::iterator iter;
        for(int xx = -length-1; xx <= length+1; xx++)
        {
            for(int yy = -width-1; yy <= width+1; yy++)
            {
                int nx = x + xx, ny = y + yy;
                if(nx >=0 && nx < sx && ny >=0 && ny < sy)
                {
                    idx = costmap->toIndex(nx, ny);
                    iter = footprint_cells.find(idx);
                    is_inflate = abs(xx) > half_length || abs(yy) > half_width;
                    if(iter == footprint_cells.end())
                    {
                        // //计算旋转角度下的足迹
                        // int rotate_x = round((nx-x)*cos(theta) - (ny-y)*sin(theta) + x);
                        // int rotate_y = round((nx-x)*sin(theta) + (ny-y)*cos(theta) + y);
                        int rotate_x = nx;
                        int rotate_y = ny;
                        footprint_cells[idx] = FootprintCell(rotate_x, rotate_y, is_inflate);
                    }
                    else if(!is_inflate)
                    {
                        iter->second.inflate = false;
                    }
                }
            }
        }

        // if(footprint_cells.size() != (length*2+1)*(width*2+1) ){
        //     WARNING("footprintf is less,count:"<<footprint_cells.size()<<" len:"<<length<<" width:"<<width);
        // }

        return true;
    }


    bool Vehicle::getFootprintOccupiedCells(const CostMapPtr &costmap, const NodePtr &node, std::map<int, FootprintCell> &footprint_cells)
    {
        if(costmap == nullptr)
            return false;
        int half_length = ceil(length_/costmap->getResolution()/2);
        int half_width = ceil(width_/costmap->getResolution()/2);
        int length = ceil(length_/costmap->getResolution()), width = ceil(width_/costmap->getResolution());
        int sx = costmap->getSizeInCellsX(), sy = costmap->getSizeInCellsY();
        if(node->x < half_length || node->x >= sx - half_length || node->y < half_width || node->y >= sy - half_width)
        {
            return false;
        }

        return getFootprintOccupiedCells(costmap, node->x, node->y, footprint_cells);
    }

    bool Vehicle::getFootprintOccupiedCells(const CostMapPtr &costmap, const VehicleState &state, std::map<int, FootprintCell> &footprint_cells)
    {
        if(costmap == nullptr)
            return false;
        int nx, ny;
        costmap->worldToMap(state.x, state.y, nx, ny);
        
        return getFootprintOccupiedCells(costmap, nx, ny, footprint_cells,state.th);
    }

    bool Vehicle::getPathFootprintOccupiedCells(const CostMapPtr &costmap, const NodePtr &a, const NodePtr &b, std::map<int, FootprintCell> &footprint_cells)
    {
        if(costmap == nullptr)
            return false;
        ERROR("ERROR calling getPathFootprintOccupiedCells() in vehicle base class");
        return false;
    }

    void Vehicle::updateCurFootprint(const CostMapPtr &costmap){
        if(costmap == nullptr)
            return;
        getFootprintOccupiedCells(costmap,state_,cur_footprint_cell_);
    }

    void Vehicle::updatePose(double x, double y, double th)
    {
        state_.x = x;
        state_.y = y;
        state_.th = th;


    }

    void Vehicle::getPose(VehicleState &state)
    {
        state = state_;
    }

    VehicleState Vehicle::getPose()
    {
        return state_;
    }

    void Vehicle::getNearNodes(const VehicleState &state, std::vector<Node> &nodes)
    {
        
    }

    void Vehicle::constructPlanFromGraphPath(MoyingMapPtr moyingmap, const VehicleState &start, const VehicleState &goal, const std::vector<Node>& node_path, PlanningResult &result)
    {
        if(moyingmap == nullptr)
            return;
        ERROR("ERROR calling constructPlanFromGraphPath() directly in vehicle base class");
    }

    unsigned int Vehicle::getStartNodeIndex(const VehicleState &start_state, char &sm)
    {
        ERROR("ERROR calling getStartNodeIndex() directly in vehicle base class");
    }
    unsigned int Vehicle::getGoalNodeIndex(const VehicleState &start_state, char &sm)
    {
        ERROR("ERROR calling getGoalNodeIndex() directly in vehicle base class");
    }

    bool Vehicle::isRobotCanNav(){

        sch::RobotPtr robot_ptr = sch::SystemManagerCommuClient::instance()->getRobotById(id_);
        if(robot_ptr == nullptr){
            // ERROR("SystemManagerCommuClient can't find robot of id "<<id_ );
            return false;
        }

        if( IS_CHARGING(*robot_ptr)             //充电中
            || IS_EMERGENCY(*robot_ptr)           //急停按下
            || !IS_LOCATE_SUCCESS(*robot_ptr)     //定位成功
            || !IS_SELF_CHECK_SUCCESS(*robot_ptr) //自检成功
        ){
            return false;
        }else
        {
            return true;
        }

    }

    bool Vehicle::isRobotInEmergencyObstacle(){

        sch::RobotPtr robot_ptr = sch::SystemManagerCommuClient::instance()->getRobotById(id_);
        if(robot_ptr == nullptr){
            // ERROR("SystemManagerCommuClient can't find robot of id "<<robot_ptr );
            return false;
        }
        return IS_EMERGENCY_OBSTACLE(*robot_ptr);
        
    }

    void Vehicle::set_obstacle_info(const LocalObstacleInfo& obs_info){

        std::lock_guard<std::recursive_mutex> lock(obstacle_info_mutex_);
        obstacle_info_ = obs_info;
    }

    const LocalObstacleInfo& Vehicle::obstacle_info(){

        std::lock_guard<std::recursive_mutex> lock(obstacle_info_mutex_);
        return obstacle_info_;
    }
}
}